#include <ros/ros.h>
#include<WarbotsMsg/velocityMsg.h>
#include<WarbotsMsg/posUpdateMsg.h>
#include <math.h>
#include "common.h"

#define CORR_FACTOR 2.3

// function prototype
void encoderCallback(const WarbotsMsg::velocityMsg& vMsg);


double x = 0;
double y = 0;
double theta = 0;
double dt = 1.0*CORR_FACTOR/CAMERA_LOOP_RATE;

WarbotsMsg::posUpdateMsg pMsg;

void encoderCallback(const WarbotsMsg::velocityMsg& vMsg)
{
   double x_local, y_local, theta_local;
   x_local = 0.25*RADIUS_OF_WHEELS*(-cos(0.785)*vMsg.v1-cos(0.785)*vMsg.v2+cos(0.785)*vMsg.v3+cos(0.785)*vMsg.v4);
   y_local = 0.25*RADIUS_OF_WHEELS*(sin(0.785)*vMsg.v1-sin(0.785)*vMsg.v2-sin(0.785)*vMsg.v3+sin(0.785)*vMsg.v4);
   theta_local = 0.25*RADIUS_OF_WHEELS*(vMsg.v1+vMsg.v2+vMsg.v3+vMsg.v4)/LENGTH;

   pMsg.x = x = x + dt*(cos(theta)*x_local - sin(theta)*y_local);
   pMsg.y = y = y + dt*(sin(theta)*x_local + cos(theta)*y_local);
   pMsg.theta = theta = theta + dt*theta_local;

}

int main(int argc, char **argv)
{
   ros::init(argc, argv, "camera_sim");
   ros::start();
   ros::Rate loop_rate(CAMERA_LOOP_RATE);
   ros::Rate poll_rate(POLL_RATE);
   ros::NodeHandle n_camera, n_encoder;
   ros::Publisher pos_pub = n_camera.advertise<WarbotsMsg::posUpdateMsg>("position_update", 1);
   ros::Subscriber encoder_sub = n_encoder.subscribe("act_mRot_rpm", 1, encoderCallback);

   while( pos_pub.getNumSubscribers() == 0 )
   {
      poll_rate.sleep();
   }

   // Initialize the robot's position
   pMsg.x = x;
   pMsg.y = y;
   pMsg.theta = theta;

   while( pos_pub.getNumSubscribers() == 0 )
   {
      poll_rate.sleep();
   }

   while( ros::ok() )
   {
      pos_pub.publish(pMsg);
      ros::spinOnce();
      loop_rate.sleep();
   }

   return 0;
}
